//
// Created by 11401 on 25-7-7.
//

#include "Servos.h"
#include "fashion_star_uart_servo.h"
#include "fashion_star_uart_servo_examples.h"
#include "usart.h"


/**
 * @brief 查看舵机是否在线
 *
 * This function iterates through servos 0 to 4, pings each servo to check if it is online, and
 * displays the result on the LCD. If a servo is online, it updates the LCD with "servo [id] is online"
 * and toggles a specific GPIO pin. If a servo is offline, it updates the LCD with "Servo e,Error Code=[id]"
 * and toggles a different GPIO pin. The function pauses for 100 milliseconds between each check.
 */

void FSUSExample_debug(void)
{
    FSUS_STATUS status_code; // 状态码
    uint8_t servo_id = 0;

    uint8_t sead[25];
    // 舵机通信检测
    for(uint8_t i=0;i<=4;i++)
    {
        status_code = FSUS_Ping(servo_usart, i);
        if (status_code== FSUS_STATUS_SUCCESS)
        {

            //在线
            sprintf(sead,(char*)"servo %d is online", i);
            ILI9341_DispStringLine_EN(LINE(i+2),(char*)sead);
            memchr(sead,0,sizeof(sead));
            HAL_GPIO_WritePin(GPIOB,GPIO_PIN_0,0);
        }
        else
        {
            //离线
            sprintf(sead,(char*)"Servo e,Error Code=%d", i);
            ILI9341_DispStringLine_EN(LINE(i+2),(char*)sead);
            memchr(sead,0,sizeof(sead));
            HAL_GPIO_WritePin(GPIOB,GPIO_PIN_5,0);
        }
        HAL_Delay(100);
    }
}


/* 控制单个舵机的角度 */
void FSUS_SetServoAngle_one(uint8_t servo_id, float angle/*取值在-180~180*/)
{
    FSUS_STATUS status_code;
    uint8_t sead[25];
    uint8_t flag=1;
    // 舵机控制相关的参数
    // 舵机的ID号
    // 舵机的目标角度
    uint16_t interval=1000;
    uint16_t power=0;
    float angle_read;
    float err;
    FSUS_SetServoAngle(servo_usart, servo_id, angle, interval, power);
    do
    {
        FSUS_QueryServoAngle(servo_usart, servo_id, &angle_read);
        sprintf(sead,(char*)"Servo %d angle=%d   ", servo_id,(int)angle_read);
        ILI9341_DispStringLine_EN(LINE(servo_id+3+4),(char*)sead);
        err=angle-angle_read;

        if (err<0.2)
        {
            if (err>-0.2)
            {
                flag=0;
            }
        }
    }while(flag);
}

/**
 * @brief 设置舵机角度
 *
 * This function sets the specified angle for each of the four servos with a fixed interval
 * and power. After setting the angles, it waits for 1 second, then queries and displays
 * the current angles of the servos on the LCD.
 *
 * @param servo_1_angle The target angle for servo 1.
 * @param servo_2_angle The target angle for servo 2.
 * @param servo_3_angle The target angle for servo 3.
 * @param servo_4_angle The target angle for servo 4.
 */
void FSUSExample_SetNServoAngle_all(float servo_1_angle,
                                    float servo_2_angle,
                                    float servo_3_angle,
                                    float servo_4_angle)
{
    //// 舵机控制相关的参数
    // 时间间隔ms
    uint8_t sead[25];
    float angle_read;
    float err;
    // 可以尝试修改设置更小的时间间隔，例如500ms
    uint16_t interval = 2000;
    // 舵机执行功率 mV 默认为0
    uint16_t power = 0;
    // 控制舵机云台角度
    FSUS_SetServoAngle(servo_usart, 0, servo_1_angle, interval, power);
    FSUS_SetServoAngle(servo_usart, 1, servo_2_angle, interval, power);
    FSUS_SetServoAngle(servo_usart, 2, servo_3_angle, interval, power);
    FSUS_SetServoAngle(servo_usart, 3, servo_4_angle, interval, power);

    HAL_Delay(1000);
    do
    {
        for (uint8_t i=0;i<=4;i++)
        {
            FSUS_QueryServoAngle(servo_usart, i, &angle_read);
            sprintf(sead,"Servo %d angle=%d  ",i,(int)angle_read);
            ILI9341_DispStringLine_EN(LINE(i+3+4),(char*)sead);
        }
    }while (0);
}

/**
 * @brief  轮转模式
 *
 * This function sets the linear actuator to rotate in a clockwise or counterclockwise direction
 * for a specified number of circles. It then queries the servo's current angle and displays it on an LCD.
 * The process is repeated 10 times with a 100-millisecond delay between each query.
 *
 * @param is_cw Direction of rotation: 1 for clockwise, 0 for counterclockwise.
 * @param n_circle Number of full circles the actuator should rotate.
 */
void  Linear_Actuator(uint8_t is_cw,uint16_t n_circle)
{
    FSUS_STATUS status_code;
    uint8_t sead[25];
    uint8_t flag=10;
    // 舵机控制相关的参数
    // 舵机的ID号
    uint8_t servo_id=0;
    // 舵机的目标角度
    uint16_t interval=1000;
    uint16_t power=0;
    float angle_read;
    float err;
    //控制舵机多圈
    //前面可以加用于启动
    //FSUS_SetServoAngle_one(0, -100.0);
    FSUSExample_WheelMoveNCircle(is_cw,n_circle);
    do
    {
        FSUS_QueryServoAngle(servo_usart, servo_id, &angle_read);
        sprintf(sead,"Linear_Actuator=%d",(int)angle_read);
        ILI9341_DispStringLine_EN(LINE(4+4),(char*)sead);
        HAL_Delay(100);
    }while (flag--);
}

//请写一个两数相乘的函数



